#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,0.5,0.05,0, DIRECT);

double kpmodel = 1, taup = 50, theta[30];
const double outputStart = 50;
const double inputStart=200;
int tindex=0;
unsigned long modeltime=0, serialtime=0;

void DoModel()
{
 
  //Create a dead time by using a circular buffer
  theta[tindex] = Output; 
  tindex++;
  if(tindex>29)tindex=0;

  // Compute the input
  //Input = (kpmodel / taup) *(theta[tindex]-outputStart) + (Input-inputStart)*(1-1/taup)+inputStart + ((float)random(-10,10))/100;
  Input = (kpmodel / taup) *(theta[tindex]-outputStart) + Input/*(Input-inputStart)*(1-1/taup)+ inputStart*/ + ((float)random(-10,10))/100;

}

void setup()
{
  Input = inputStart;
  for(int i=0;i<30;i++)theta[i] = outputStart;
  
  //initialize the variables we're linked to
  Setpoint = 200;
Output = outputStart;
  myPID.SetOutputLimits(-250,250);
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  
  Serial.begin(9600);
  Serial.println(theta[0]);
}

void loop()
{
  
  
  unsigned long now = millis();
  
  if(now>120000)while(true){}
  if(modeltime<now)
  {
     modeltime+=100; 
     DoModel();  
     
  }
  myPID.Compute();

  
  //setpoint stepper
/*  if(now>110000) Setpoint = 100;
  else*/ if(now>60000) Setpoint = 200;
  else if(now>20000)Setpoint=150;
  /*else if(now>30000)Setpoint=50;*/
  
  
  //random mode changes
  if(now>110000) myPID.SetMode(AUTOMATIC);
  else if(now>109000) myPID.SetMode(MANUAL);
  else if(now>69000) myPID.SetMode(AUTOMATIC);
  else if(now>68000) myPID.SetMode(MANUAL);
  else if(now>45000) myPID.SetMode(AUTOMATIC);  
  else if(now>4000) myPID.SetMode(AUTOMATIC);  
  
/*  if(now>80000) myPID.SetTunings(0.25,0.125,0.05);
  else if(now>40000) myPID.SetTunings(0.75,0.35,0.15);*/

  
  
  if(serialtime<now)
  {
     serialtime+=100; 
     Serial.print(now); Serial.print(" ");
     Serial.print(Setpoint); Serial.print(" ");
     Serial.print(Input); Serial.print(" ");
     Serial.println(Output);
  }
  
}






